#include <config.h>
#include <asm/regdef.h>
#include <asm/mipsregs.h>
#include <asm/addrspace.h>
#include <asm/asm.h>

#include "ns16550.h"

#include "loongson3_def.h"
#include "ls7a_config.h"

#include "ls7a/ls7a_define.h"
#include "ls7a/ht.h"

	.text
	.set noreorder
	.set mips64

	.globl	lowlevel_init
lowlevel_init:
	move fp, ra

	//CP0 Register 16, Select 6
	//处理器store操作自动写合并功能使能位
	mfc0  t0, $16, 6
	or    t0, 0x100
	xori  t0, 0x100
	mtc0  t0, $16, 6
	//该位复位后必须对其写1，且不再更改为0
	mfc0  t0, $16, 6
	ori   t0, 0x200
	mtc0  t0, $16, 6

	//CP0 Register 22, Select 0
	mfc0  t0, $22
	lui   t1, 0x0000
//	lui   t1, 0x8000
	or    t0, t1, t0
	mtc0  t0, $22

//	.set mips0

	lui  t1, 0x40    //set bev to 1'b1
	ori  t1, 0x00e0  //set {UX,SX,KX} to 3'b111
	mtc0 t1, CP0_STATUS
	mtc0 zero, CP0_CAUSE



	/* WatchDog chip MAX6369 disable work */
	WatchDog_Close

	/* spi speedup */
	li  t0, 0xbfe00220
	li  t1, 0x07
	sb  t1, 0x4(t0)

	/*
	 *  We get here from executing a bal to get the PC value of the current execute
	 *  location into ra. Check to see if we run from ROM or if this is ramloaded.
	 */
locate:
	move	s0, zero


	/* here we get l2 cache initialized */
	.set  mips64
	mfc0  t0, $15, 1
	andi  t0, 0x3ff
	dli   a0, 0x9800000000000000
	andi  t1, t0, 0x3		/* core id */
	dsll  t2, t1, 18
	or    a0, t2, a0		/* 256KB offset for the each core */
	andi  t2, t0, 0xc		/* node id */
	dsll  t2, 42
	or    a0, t2, a0		/* get the L2 cache address */

	dsll  t1, t1, 8
	or    t1, t2, t1

	dli   t2, NODE0_CORE0_BUF0
	or    t1, t2, t1

	li    t3, RESERVED_COREMASK
	andi  t3, 0xf
	li    t1, 0x1
	sllv  t1, t1, t0
	and   t3, t1, t3
	bnez  t3, wait_to_be_killed
	nop
	li    t2, BOOTCORE_ID
	bne   t0, t2, 1f
	nop
	lui   v0, 0xbfe0
	addiu v0, 0x01d0
	lw    t2, 0x0(v0)
	xori  t2, SHUTDOWN_MASK
	sw    t2, 0x0(v0)

	b     1f
	nop

wait_to_be_killed:
	b     wait_to_be_killed
	nop
1:
	dli   a0, BOOTCORE_ID
	bne   t0, a0, slave_main
	nop

	bal   initserial
	nop
	bal   clear_mailbox
	nop

//#define SHUT_SLAVES
#ifdef SHUT_SLAVES
	PRINTSTR("\r\nShut down other cores\r\n")
	li    a0, 0xbfe001d0
	li    a1, BOOTCORE_ID
	sll   a1, 2
	li    t1, 0xf
	sll   a1, t1, a1
	li    t1, 0x88888888
	or    t1, a1, t1
	sw    t1, 0x0(a0)
	li    t1, 0x00000000
	or    t1, a1, t1
	sw    t1, 0x0(a0)
#else
	PRINTSTR("\r\nNOT Shut down other cores\r\n")
#endif

bsp_start:
	PRINTSTR("\r\nMIPS Initializing. Standby...\r\n")
	/*
	 * Now determine DRAM configuration and size by
	 * reading the I2C EEROM on the DIMMS
	 */

##############################################
	/*
	 * now, we just write ddr2 parameters directly.
	 * we should use i2c for memory auto detecting.
	 */
	//Read sys_clk_sel
	TTYDBG ("\r\n0xbfe00190  : ")
	li    t2, 0xbfe00190
	ld    t1, 0x0(t2)
	dsrl  a0, t1, 32
	bal   print_hex
	nop
	move  a0, t1
	bal   print_hex
	nop
	TTYDBG ("\r\nCPU CLK SEL : ")
	dsrl  t1, t1, 32
	andi  a0, t1, 0x1f
	bal   print_hex
	nop
	TTYDBG ("\r\nMEM CLK SEL : ")
	dsrl  t0, t1, 5
	andi  a0, t0, 0x1f
	bal   print_hex
	nop
	TTYDBG ("\r\nHT CLK SEL : ")
	dsrl  t0, t1, 10
	andi  a0, t0, 0x3f
	bal   print_hex
	nop
	TTYDBG ("\r\n")

//USING S1 FOR PASSING THE NODE ID
	dli   s1, 0X0000000000000000
#include "loongson3_clksetting.S"
//#include "loongson3a8_clk.S"

##########################################

#include "loongson3_fixup.S"
#ifdef MULTI_CHIP
//USING S1 FOR PASSING THE NODE ID
	dli  s1, 0x0000100000000000
#include "loongson3_clksetting.S"
#endif

##########################################
	PRINTSTR("NO TLB cache init ...\r\n")

#include "pcitlb.S" /* map 0x4000000-0x7fffffff to 0xc0000000 */

/*
 *  Reset and initialize l1 caches to a known state.
 */
#if 1
	## enable kseg0 cachablilty####
	mfc0  t6, CP0_CONFIG
	ori   t6, t6, 7
	xori  t6, t6, 4
	mtc0  t6, CP0_CONFIG

	#jump to cached kseg0 address
	PRINTSTR("Jump to 9fc\r\n")
	lui   t0, 0xdfff
	ori   t0, t0, 0xffff
	bal   1f
	nop
1:
	and   ra, ra, t0
	addiu ra, ra, 16
	jr    ra
	nop
#endif
	TTYDBG("32 bit PCI space translate to 64 bit HT space\r\n")
#include "loongson3_ht1_32addr_trans.S"

	//config fix address bar for Misc devices block
	dli     t0, MISC_HEADER_ADDR
	li      t1, MISC_BASE_ADDR
	sw      t1, 0x10(t0)
	lw      t2, 0x4(t0)
	ori     t2, t2, 0x2
	sw      t2, 0x4(t0)
	//change confbus base address
	dli     t0, CONFBUS_HEADER_ADDR
	li      t1, CONFBUS_BASE_ADDR
	sw      t1, 0x10(t0)
	lw      t2, 0x4(t0)
	ori     t2, t2, 0x2
	sw      t2, 0x4(t0)
	TTYDBG("set LS7A MISC and confbus base address done.\r\n")

#ifdef CONFIG_SB_LS7A1000_INIT_ASM //disable 7a link
	//setup LS3A - 7A HT link start...
	//check 3A clksel setting
	li      t0, 0xbfe00190
	lw      a0, 0x4(t0)
	srl     a0, a0, 15
	beqz    a0, 3f
	nop
#ifdef  CHECK_HT_PLL_MODE
	TTYDBG("Warning: 3A HT in hard freq mode, please modify clksel[7].\r\n")
	dli     a0, 0x4000000
1:
	dsub    a0, a0, 1
	bnez    a0, 1b
	nop
#endif
	b       2f
	nop
3:
	TTYDBG("3A HT in soft freq cfg mode...ok\r\n")
2:

	//check 7A clksel setting
	dli     t0, (LS7A_MISC_BASE_ADDR + 0x60000)//gpio基地址
	lb      a0, (0xa00+53)(t0)//读取SYS_CLKSEL7（gpio53）,HT频率配置模式
	beqz    a0, 3f
	nop
#ifdef  CHECK_HT_PLL_MODE
	TTYDBG("Warning: 7A HT in hard freq mode, please modify clksel[7].\r\n")
	dli     a0, 0x4000000
1:
	dsub    a0, a0, 1
	bnez    a0, 1b
	nop
#endif
	b       2f
	nop
3:
	TTYDBG("7A HT in soft freq cfg mode...ok\r\n")
2:

	li      t2, ((HT1_HARD_FREQ_CFG << 12) | (HT1_HARD_FREQ_CFG << 8) | (HT1_GEN_CFG << 4) | (HT1_WIDTH_CFG << 1) | (HT1_RECONNECT << 0))

	li      t8, LS7A_HT1_SOFT_FREQ_CFG
	dsll    t3, t8, 32
	li      t8, LS3A_HT1_SOFT_FREQ_CFG
	or      t3, t3, t8

#ifdef DEBUG_HT1
	PRINTSTR("HT1 default setting: \r\na1: 0x")
	move    a0, t2
	bal     print_hex
	nop
	PRINTSTR("\r\na2: 0x")
	dsrl    a0, t3, 32
	bal     print_hex
	nop
	move    a0, t3
	bal     print_hex
	nop
	PRINTSTR("\r\nInput parameter a1: ([15:12]: 7A freq-0/2/5/9; [11:8]: 3A freq-0/2/5/9; [7:4]: GENx-1/3; [1]: width-0/1; [0]: reconnect-0/1): ")
	bal     inputaddress
	nop
	beqz    v0, 1f
	nop
	move    t2, v0
1:
	PRINTSTR("\r\nInput parameter a2: ([3:0]: ht pll soft cfg sel. 0: 200M; 2: 400M; 5: 800M; 6: 1000M; 7: 1200M; 9: 1600M; b: 2000M; c: 2200M; d: 2400M; e: 2600M; f: 3200M;): ")
	bal     inputaddress
	nop
	move    t1, v0
	PRINTSTR("\r\n")

	move    a0, $0
	dli     t3, ((LS7A_HT_PLL_200M | 0x2) << 32) | (LS3A_HT_PLL_200M | 0x2)    //0
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 2
	dli     t3, ((LS7A_HT_PLL_400M | 0x2) << 32) | (LS3A_HT_PLL_400M | 0x2)    //2
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 3
	dli     t3, ((LS7A_HT_PLL_800M | 0x2) << 32) | (LS3A_HT_PLL_800M | 0x2)    //5
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 1
	dli     t3, ((LS7A_HT_PLL_1000M | 0x2) << 32) | (LS3A_HT_PLL_1000M | 0x2)   //6
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 1
	dli     t3, ((LS7A_HT_PLL_1200M | 0x2) << 32) | (LS3A_HT_PLL_1200M | 0x2)   //7
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 2
	dli     t3, ((LS7A_HT_PLL_1600M | 0x2) << 32) | (LS3A_HT_PLL_1600M | 0x2)   //9
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 2
	dli     t3, ((LS7A_HT_PLL_2000M | 0x2) << 32) | (LS3A_HT_PLL_2000M | 0x2)   //b
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 1
	dli     t3, ((LS7A_HT_PLL_2200M | 0x2) << 32) | (LS3A_HT_PLL_2200M | 0x2)   //c
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 1
	dli     t3, ((LS7A_HT_PLL_2400M | 0x2) << 32) | (LS3A_HT_PLL_2400M | 0x2)   //d
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 1
	dli     t3, ((LS7A_HT_PLL_2600M | 0x2) << 32) | (LS3A_HT_PLL_2600M | 0x2)   //e
	beq     t1, a0, 8f
	nop
	daddu   a0, a0, 1
	dli     t3, ((LS7A_HT_PLL_3200M | 0x2) << 32) | (LS3A_HT_PLL_3200M | 0x2)   //f
	beq     t1, a0, 8f
	nop
	bgt     t1, a0, 2f
	nop

	PRINTSTR("Error: freq select illegle, use default 800M.")
	dli     t3, ((LS7A_HT_PLL_800M | 0x2) << 32) | (LS3A_HT_PLL_800M | 0x2)    //5
	b       8f
	nop
2:
	move    t3, t1
8:
#endif

	WatchDog_Enable
	dli     a0, 0x90000e0000000000
	move    a1, t2
	move    a2, t3
	bal     config_ht_link
	nop
#ifdef  LS7A_2WAY_CONNECT
	dli     a0, 0x90001e0000000000
	move    a1, t2
	move    a2, t3
	bal     config_ht_link
	nop
#endif

	//WatchDog_Enable

	move    a1, t2
	bal     reset_ht_link
	nop
	li      a0, 0xf3f3
	and     a0, a0, v0
	beqz    a0, linkup
	nop
	move    t8, v0
	TTYDBG("!!!LS3A-7A link error occur. Error status: ")
	move    a0, t8
	bal     print_hex
	nop
linkup:
	TTYDBG("LS3A-7A linkup.")

#endif //CONFIG_SB_LS7A1000_INIT_ASM

//##########################################
//DDR config start
//cxk
####################################
#include "mm/lsmc_ddr_param_define.h"
#include "mm/ddr_config_define.h"
//#define DDR_DLL_BYPASS
#define DISABLE_DIMM_ECC
#define PRINT_MSG
#ifndef ARB_LEVEL
//#define FIX_DDR_PARAM
#endif
#ifdef  ARB_LEVEL
#define AUTO_ARB_LEVEL
#endif
#ifdef  AUTO_ARB_LEVEL
//#define CHECK_ARB_LEVEL_FREQ
#ifdef  AUTO_DDR_CONFIG
#define CHECK_ARB_LEVEL_DIMM
#endif
//#define DEBUG_AUTO_ARB_LEVEL
#endif
//#define  DISABLE_DDR_A15
//#define DEBUG_DDR
//#define DEBUG_DDR_PARAM
//#define DISABLE_HARD_LEVELING
//#define DLL_DELAY_LOOP
//#define PRINT_DDR_LEVELING
//#define DLL_CK_DELAY_DEBUG
//#define NO_AUTO_TRFC  //adjust TRFC param manually if defined
//#define PRINT_MSG
//#define PRINT_DDR_LEVELING
//#define DEBUG_GMEM
//#define DEBUG_GMEM_PARAM


	TTYDBG("\r\nStart Init Memory, wait a while......\r\n")
####################################
	move    msize, $0
	move    s3, $0
	//!!!!important--s1 must be correctly set

	TTYDBG("NODE 0 MEMORY CONFIG BEGIN\r\n")

#ifdef  AUTO_DDR_CONFIG
	dli     s1, 0xf1f00000  //set use MC1 or MC0 or MC1/0 and give All device id
#else
#ifndef DDR_S1
	dli     s1, 0xf0a31000f0a31004  // use both, 8G SCS RDIMM
#else
#if 0
    dli     a0, ( MC_SDRAM_TYPE_DDR3    /* sdram type: DDR3/DDR2 */ \
                | MC_DIMM_ECC_NO        /* dimm ECC: YES/NO */ \
                | MC_DIMM_BUF_REG_NO    /* dimm buffer register: YES/NO, for RDIMM use YES, all else use NO*/ \
                | MC_DIMM_WIDTH_64      /* memory data width: 64/32 */ \
                | MC_SDRAM_ROW_15       /* sdram row address number: 15~11 */ \
                | MC_SDRAM_COL_10       /* sdram column address number: 12~9 */ \
                | MC_SDRAM_BANK_8       /* sdram bank number: 8/4 */ \
                | MC_ADDR_MIRROR_YES    /* for standard DDR3 UDIMM, use YES else use NO */ \
                | MC_SDRAM_WIDTH_X8     /* SDRAM device data width: 8/16 */ \
                | MC_USE_CS_0_1         /* the CS pins the sdram connected on(split by '_', from small to big) */ \
                | MC_MEMSIZE_(8)        /* MC memory size, unit: 512MB */ \
		|USE_MC_0_1 \
                )
    dsll    s1, a0, 32
    //set MC0 dimm infor
    dli     a0, ( MC_SDRAM_TYPE_DDR3    /* sdram type: DDR3/DDR2 */ \
                | MC_DIMM_ECC_NO        /* dimm ECC: YES/NO */ \
                | MC_DIMM_BUF_REG_NO    /* dimm buffer register: YES/NO, for RDIMM use YES, all else use NO*/ \
                | MC_DIMM_WIDTH_64      /* memory data width: 64/32 */ \
                | MC_SDRAM_ROW_15       /* sdram row address number: 15~11 */ \
                | MC_SDRAM_COL_10       /* sdram column address number: 12~9 */ \
                | MC_SDRAM_BANK_8       /* sdram bank number: 8/4 */ \
                | MC_ADDR_MIRROR_YES    /* for standard DDR3 UDIMM, use YES, else use NO */ \
                | MC_SDRAM_WIDTH_X8     /* SDRAM device data width: 8/16 */ \
                | MC_USE_CS_0_1         /* the CS pins the sdram connected on(split by '_', from small to big) */ \
                | MC_MEMSIZE_(8)        /* MC memory size, unit: 512MB */ \
		| USE_MC_0_1 \
                )
    or      s1, s1, a0
    //set used MC and NODE ID
    or      s1, s1, MC_NODE_ID_0        /* node ID: 0/1/2/3 */
#endif
	dli 	s1, DDR_S1
#endif //DDR_S1
#endif //AUTO_DDR_CONFIG

#include "mm/loongson3A2000_ddr2_config.S"

/* test memory */
	dli     t0, 0x9000000000000000
	dli     a0, 0x5555555555555555
	sd      a0, 0x0(t0)
	dli     a0, 0xaaaaaaaaaaaaaaaa
	sd      a0, 0x8(t0)
	dli     a0, 0x3333333333333333
	sd      a0, 0x10(t0)
	dli     a0, 0xcccccccccccccccc
	sd      a0, 0x18(t0)
	dli     a0, 0x7777777777777777
	sd      a0, 0x20(t0)
	dli     a0, 0x8888888888888888
	sd      a0, 0x28(t0)
	dli     a0, 0x1111111111111111
	sd      a0, 0x30(t0)
	dli     a0, 0xeeeeeeeeeeeeeeee
	sd      a0, 0x38(t0)


	PRINTSTR("The uncache data is:\r\n")
	dli     t1, 8
	dli     t5, 0x9000000000000000
1:
	ld      t6, 0x0(t5)
	move    a0, t5
	and     a0, a0, 0xfff
	bal     print_hex
	nop
	PRINTSTR(":  ")
	dsrl    a0, t6, 32
	bal     print_hex
	nop
	move    a0, t6
	bal     print_hex
	nop
	PRINTSTR("\r\n")

	daddiu  t1, t1, -1
	daddiu  t5, t5, 8
	bnez    t1, 1b
	nop

	PRINTSTR("The cached  data is:\r\n")
	dli     t1, 8
	dli     t5, 0x9800000000000000
1:
	ld      t6, 0x0(t5)
	move    a0, t5
	and     a0, a0, 0xfff
	bal     print_hex
	nop
	PRINTSTR(":  ")
	dsrl    a0, t6, 32
	bal     print_hex
	nop
	move    a0, t6
	bal     print_hex
	nop
	PRINTSTR("\r\n")

	daddiu  t1, t1, -1
	daddiu  t5, t5, 8
	bnez    t1, 1b
	nop

#ifdef MULTI_CHIP
	TTYDBG("NODE 1 MEMORY CONFIG BEGIN\r\n")

#ifdef AUTO_DDR_CONFIG
	dli     s1, 0xf3f20001  //set use MC1 or MC0 or MC1/0 and give All device id
#else
//	dli     s1, 0xc2e30400c2e30405
	dli     s1, 0xf0a31000f0a31001  // use both, 8G SCS RDIMM
#endif

#include "ddr_dir/loongson3A2000_ddr2_config.S"

/* test memory */
//TEST if NODE1 has a memory
	dli     t0, 0x9000100000000008
	lw      a0, 0x0(t0)
	li      a1, 0x10000000
	beq     a0, a1, 11f
	nop

	dli     t0, 0x9000100000000000
	dli     a0, 0x5555555555555555
	sd      a0, 0x0(t0)
	dli     a0, 0xaaaaaaaaaaaaaaaa
	sd      a0, 0x8(t0)
	dli     a0, 0x3333333333333333
	sd      a0, 0x10(t0)
	dli     a0, 0xcccccccccccccccc
	sd      a0, 0x18(t0)
	dli     a0, 0x7777777777777777
	sd      a0, 0x20(t0)
	dli     a0, 0x8888888888888888
	sd      a0, 0x28(t0)
	dli     a0, 0x1111111111111111
	sd      a0, 0x30(t0)
	dli     a0, 0xeeeeeeeeeeeeeeee
	sd      a0, 0x38(t0)

	PRINTSTR("The uncache data is:\r\n")
	dli     t1, 8
	dli     t5, 0x9000100000000000
1:
	ld      t6, 0x0(t5)
	move    a0, t5
	and     a0, a0, 0xfff
	bal     print_hex
	nop
	PRINTSTR(":  ")
	dsrl    a0, t6, 32
	bal     print_hex
	nop
	move    a0, t6
	bal     print_hex
	nop
	PRINTSTR("\r\n")

	daddiu  t1, t1, -1
	daddiu  t5, t5, 8
	bnez    t1, 1b
	nop

	PRINTSTR("The cached  data is:\r\n")
	dli     t1, 8
	dli     t5, 0x9800100000000000
1:
	ld      t6, 0x0(t5)
	move    a0, t5
	and     a0, a0, 0xfff
	bal     print_hex
	nop
	PRINTSTR(":  ")
	dsrl    a0, t6, 32
	bal     print_hex
	nop
	move    a0, t6
	bal     print_hex
	nop
	PRINTSTR("\r\n")

	daddiu  t1, t1, -1
	daddiu  t5, t5, 8
	bnez    t1, 1b
	nop

11:
#endif //MULTI_CHIP

	/*judge the node1 whether have memory*/
	and     a0, msize, 0xff
//	beqz    a0, beep_on
//	nop


	TTYDBG("Init Memory done.\r\n")

#if 0   //test memory
	dli     s1, 0x0001000080000000
	dli     t1, 0x0010
	bal     test_mem
	nop
	move    t1, v0
	PRINTSTR("\r\n")
	dsrl    a0, t1, 32
	bal     print_hex
	nop
	move    a0, t1
	bal     print_hex
	nop
	beqz    t1, 2f
	nop
	PRINTSTR("  Error found!!\r\n")

	bal     beep_on
	nop
1:
	b       1b
	nop
2:
#endif
##########################################

#ifdef DEBUG_DDR
#ifdef DEBUG_DDR_PARAM
	PRINTSTR("\r\nDo test?(0xf: skip): ")
	bal     inputaddress
	nop
	and     v0, v0, 0xf
	dli     a1, 0x1
	bgt     v0, a1, 3f
	nop
#endif

	dli     s1, 0x0010000080000000
#ifdef DEBUG_DDR_PARAM
	PRINTSTR("\r\ndefault s1 = 0x");
	dsrl    a0, s1, 32
	bal     print_hex
	nop
	PRINTSTR("__")
	move    a0, s1
	bal     print_hex
	nop
	PRINTSTR("\r\nChange test param s1(0: skip)?: ")
	bal     inputaddress
	nop
	beqz    v0, 1f
	nop
	move    s1, v0
1:
#endif
1:
	dli     t1, 0x0010
	bal     test_mem
	nop
	move    t1, v0
	PRINTSTR("\r\n")
	dsrl    a0, t1, 32
	bal     print_hex
	nop
	move    a0, t1
	bal     print_hex
	nop
	beqz    t1, 2f
	nop
	PRINTSTR("  Error found!!\r\n")
2:
#if 0
	b       1b
	nop
#endif

3:
#endif //DEBUG_DDR

#ifdef AUTO_ARB_LEVEL
#include "mm/store_auto_arb_level_info.S"
#endif
#ifdef LOCK_SCACHE
	bal lock_scache
	nop
	TTYDBG("cache lock done\r\n")
	nop
#endif
##########################################
#ifdef CONFIG_SB_LS7A1000_INIT_ASM
//Initialize LS7A here, cxk
#if 1
	TTYDBG("\r\nbridge CHIP ID: 0x")
	dli     t0, LS7A_CONFBUS_BASE_ADDR
	lw      a0, 0x3ff8(t0)
	bal     hexserial
	nop
	TTYDBG("revision: 0x")
	lw      a0, 0x3ffc(t0)
	srl     a0, a0, 24
	bal     hexserial
	nop
	TTYDBG("\r\n")

	//check chip ID
	lw      a0, 0x3ff8(t0)
	li      a1, 0x7A000000
	xor     a0, a0, a1
	srl     a0, a0, 24  //check 7A only
	beqz    a0, 2f
	nop
	bal     hexserial
	nop
	TTYDBG("\r\nbridge CHIP ID check failed!!!")
1:
	b       1b
	nop
2:
#endif

//HT1 window and configurations
	dli     a0, 0x90000e0000000000
	bal     ls3a7a_ht_init
	nop
	TTYDBG("Node 0 LS3A-7A init done.\r\n")
#ifdef  LS7A_2WAY_CONNECT
	dli     a0, 0x90001e0000000000
	bal     ls3a7a_ht_init
	nop
	TTYDBG("Node 1 LS3A-7A init done.\r\n")
#endif
#if 1
#include "ls7a/ls7a_dbg.S"
#endif
#include "ls7a/ls7a_init.S"
	TTYDBG("\r\nLS7A init done.\r\n")
#endif //CONFIG_SB_LS7A1000_INIT_ASM
##########################################
#if 0
    dli     s1, 0x0000000000000000
#include "loongson3_debug_window.S"

#ifdef MULTI_CHIP
    dli     s1, 0x0000100000000000
#include "loongson3_debug_window.S"
#endif
#endif
###########################################

	dli   t0, NODE0_CORE0_BUF0  #buf of cpu0 we need bootcore_id
	dli   t3, BOOTCORE_ID
	dsll  t3, t3, 8
	or    t0, t0, t3
	li    t1, SYSTEM_INIT_OK
	sw    t1, FN_OFF(t0)

	move  k1, msize
	move  ra, fp
	jr    ra
	nop



LEAF(printk)
	move  a2, ra
	addu  a1, a0, s0
	lbu   a0, 0(a1)
1:
	beqz  a0, 2f
	nop
	bal   print_char
	addiu a1, 1
	b     1b
	lbu   a0, 0(a1)

2:
	move  ra, a2
	j     ra
	nop
END(printk)

LEAF(print_hex)
	move  a2, ra
	move  a1, a0
	li    a3, 7
1:
	rol   a0, a1, 4
	move  a1, a0
	and   a0, 0xf
	la    v0, hexchar
	addu  v0, s0
	addu  v0, a0
	bal   print_char
	lbu   a0, 0(v0)

	bnez  a3, 1b
	addu  a3, -1

	move  ra, a2
	j     ra
	nop
END(print_hex)

LEAF(hexserial)
	move  a2, ra
	move  a1, a0
	li    a3, 7
1:
	rol   a0, a1, 4
	move  a1, a0
	and   a0, 0xf
	la    v0, hexchar
	addu  v0, s0
	addu  v0, a0
	bal   print_char
	lbu   a0, 0(v0)

	bnez  a3, 1b
	addu  a3, -1

	move  ra, a2
	j     ra
	nop
END(hexserial)

LEAF(print_char)
	la    v0, CONFIG_SYS_NS16550_COM1
1:
	lbu   v1, NSREG(NS16550_LSR)(v0)
	and   v1, LSR_TXRDY
#	li    v1, 1
	beqz  v1, 1b
	nop
	sb    a0, NSREG(NS16550_DATA)(v0)
	move  v1, v0
	la    v0, CONFIG_SYS_NS16550_COM1
	bne   v0, v1, 1b
	nop
	jr    ra
	nop
END(print_char)

LEAF(tgt_putchar)
	la    v0, CONFIG_SYS_NS16550_COM1
1:
	lbu   v1, NSREG(NS16550_LSR)(v0)
	and   v1, LSR_TXRDY
#	li    v1, 1
	beqz  v1, 1b
	nop
	sb    a0, NSREG(NS16550_DATA)(v0)
	move  v1, v0
	la    v0, CONFIG_SYS_NS16550_COM1
	bne   v0, v1, 1b
	nop
	jr    ra
	nop
END(tgt_putchar)

LEAF(initserial)
	li a0, CONFIG_SYS_NS16550_COM1

	li t1, 128
	sb t1, 3(a0)
#ifdef BONITO_33M
	li t1, 0x12      # divider, highest possible baud rate,for 33M crystal
#endif
#ifdef BONITO_25M
	li	t1, 0x0e      # divider, highest possible baud rate,for 25M crystal
#endif
#ifdef BONITO_50M
	li	t1, 0x1b      # divider, highest possible baud rate,for 50M crystal
#endif
	sb	t1, 0(a0)
	li	t1, 0x0       # divider, highest possible baud rate
	sb	t1, 1(a0)
	li	t1, 3
	sb	t1, 3(a0)
	li	t1, 0
	sb	t1, 1(a0)
	li	t1, 71
	sb	t1, 2(a0)
	jr	ra
	nop
END(initserial)

LEAF(clear_mailbox)
	.set  mips64
	mfc0  t0, $15, 1
//	.set  mips3
	andi	t0, t0, 0x3ff
	andi	t1, t0, 0x3
	dsll	t1, 8
	andi	t2, t0, 0xc
	dsll	t2, 42
	or    t1, t2, t1
	dsrl	t2, 30              /* for 3b/3c */
	or    t1, t2, t1
	dli   t2, NODE0_CORE0_BUF0
	or    t1, t1, t2
	sd    zero, FN_OFF(t1)
	sd    zero, SP_OFF(t1)
	sd    zero, GP_OFF(t1)

	jr    ra
	sd    zero, A1_OFF(t1)
END(clear_mailbox)

LEAF(lock_scache)
#if 1
	dli  t0, 0x900010003ff04000
	dli  t1, 0xffffffffffe00000
	sd   t1, 0x240(t0)
	dli  t1, 0x8000100090000000
	sd   t1, 0x200(t0)
#if 0   //1M
	dli  t0, 0x900010003ff04000
	dli  t1, 0xfffffffffff00000
	sd   t1, 0x248(t0)
	dli  t1, 0x8000100090200000
	sd   t1, 0x208(t0)
#endif

	dli  t0, 0x900000003ff00000
	dli  t1, 0xffffffffffe00000
	sd   t1, 0x240(t0)
	dli  t1, 0x8000000090000000
	sd   t1, 0x200(t0)
#if 0   //1M
	dli  t0, 0x900000003ff00000
	dli  t1, 0xfffffffffff00000
	sd   t1, 0x248(t0)
	dli  t1, 0x8000000090200000
	sd   t1, 0x208(t0)
#endif
	jr   ra
	nop
#endif
END(lock_scache)

/********************************************************/
	.ent  slave_main
slave_main:
	dli    t2, NODE0_CORE0_BUF0
	dli    t3, BOOTCORE_ID
	dsll   t3, 8
	or     t2, t2, t3

wait_scache_allover:
	lw     t4, FN_OFF(t2)
	/* since bsp be paused, then resumed after mem initialised
	 * we need to SYSTEM_INIT_OK instead of L2_CACHE_DONE
	 */
	dli    t5, SYSTEM_INIT_OK
	bne    t4, t5, wait_scache_allover
	nop
	/**********************************************/

	## enable kseg0 cachablilty####
	mfc0   t6, CP0_CONFIG
	ori    t6, t6, 7
	xori   t6, t6, 4
	mtc0   t6, CP0_CONFIG


	#jump to cached kseg0 address
	lui    t6, 0xdfff
	ori    t6, t6, 0xffff
	bal    1f
	nop
1:
	and    ra, ra, t6
	daddiu ra, ra, 16
	jr     ra
	nop

/******************************************************************/
/* Read Mail BOX to judge whether current core can jump to kernel
 * the cpu spin till FN_OFF is NOT zero

/******************************************************************/
	/**********************
	 * t0: core ID
	 * t1: core mailbox base address
	 * t2: jump address
	 * t3: temp
	 ************************/

	bal    clear_mailbox
	nop
.global waitforinit;
waitforinit:

	li     a0, 0x1000
idle1000:
	addiu  a0, -1
	bnez   a0, idle1000
	nop

	lw     t2, FN_OFF(t1)
	beqz   t2, waitforinit
	nop

	dli    t3, 0xffffffff00000000
	or     t2, t3

	dli    t3, 0x9800000000000000
	ld     sp, SP_OFF(t1)
	or     sp, t3
	ld     gp, GP_OFF(t1)
	or     gp, t3
	ld     a1, A1_OFF(t1)

	move   ra, t2
	jr     ra  # slave core jump to kernel, byebye
	nop

	.end   slave_main
/********************************************************/

	.rdata
hexchar:
	.ascii  "0123456789abcdef"
	.text
//	.align  2

#if 1
#include "i2c_7a.S"
#ifdef AUTO_DDR_CONFIG  // for no ls2h cpu on board, disable AUTO_DDR_CONFIG
#include "mm/detect_node_dimm_all.S"
#endif
#######################################
#ifdef CONFIG_SB_LS7A1000_INIT_ASM
#include "ls7a/ls3a7a_setup_ht_link.S"
#include "ls7a/ls3a7a_ht_init.S"
#include "ls7a/ls7a_config.S"
#ifdef  LS7A_GMEM_CFG
#include "mm/ls7A_gmem_config.S"
#endif
#endif //CONFIG_SB_LS7A1000_INIT_ASM
#include "mm/ls3A8_ddr_config.S"
#ifdef DDR3_DIMM
#include "mm/loongson3C_ddr3_leveling.S"
#endif
#ifdef ARB_LEVEL
//#include "mm/ARB_level_new.S"
#endif
#if 0   // (defined(DEBUG_DDR) || defined(DEBUG_GMEM))
#include "mm/Test_Mem.S"
#endif
#######################################
#endif

	.rdata
	.global ddr2_reg_data
	.global ddr3_reg_data
	.global gmem_reg_data

	.align  5
#include "loongson_mc2_param.S"
#if  defined(LS7A_GMEM_CFG) && defined(CONFIG_SB_LS7A1000_INIT_ASM)
#include "loongson7A_gmem_param.S"
#endif

#ifdef CONFIG_32BIT
	/* u64  __raw__readq(u64 addr)
	 * a0, a1 hold low 32 and high 32
	 * v0, v1 hold low 32 and high 32 of ret
	 */
	.text
	.global __raw__readq
	.ent    __raw__readq
__raw__readq:
	dsll32  a1, a1, 0
	dsll32  a0, a0, 0
	dsrl32  a0, a0, 0
	or      a0, a1, a0

	ld      v0, 0(a0)
	dsra32  v1, v0, 0
	jr      ra
	sll     v0, v0, 0
.end    __raw__readq

	/* u64 __raw__writeq(u64 addr, u64 val)
	 * a0, a1 hold low 32 and high 32 of addr,
	 * a2, a2 hold low 32 and high 32 of val,
	 * v0, v1 hold low 32 and high 32 of ret
	 */
	.global __raw__writeq
	.ent    __raw__writeq
__raw__writeq:
	dsll32  a1, a1, 0
	dsll32  a0, a0, 0
	dsrl32  a0, a0, 0
	or      a0, a1, a0

	dsll32  a3, a3, 0
	dsll32  a2, a2, 0
	dsrl32  a2, a2, 0
	or      a2, a2, a3

	sd      a2, 0(a0)
//	ld      v0, 0(a0)
//	dsra32  v1, v0, 0
	jr      ra
//	sll     v0, v0, 0
	nop
.end    __raw__writeq

	/* u64  __raw__readw(u64 addr)
	 * a0, a1 hold low 32 and high 32
	 * v0     hold     32 of ret
	 */
	.global __raw__readw
	.ent	__raw__readw
__raw__readw:
	dsll32	a1, a1, 0
	dsll32	a0, a0, 0
	dsrl32	a0, a0, 0
	or	a0, a1, a0

	lw	v0, 0(a0)
	jr	ra
	sll	v0, v0, 0
.end	__raw__readw


	/* u64 __raw__writew(u64 addr, u32 val)
	 * a0, a1 hold low 32 and high 32 of addr,
	 * a2     hold 32 of val,
	 * v0     hold 32 of ret
	 */
	.global __raw__writew
	.ent	__raw__writew
__raw__writew:
	dsll32	a1, a1, 0
	dsll32	a0, a0, 0
	dsrl32	a0, a0, 0
	or	a0, a1, a0

	dsll32	a2, a2, 0
	dsrl32	a2, a2, 0

	sw	a2, 0(a0)
//	lw	v0, 0(a0)
	jr	ra
//	sll	v0, v0, 0
	nop
.end	__raw__writew

	/* u64  __raw__readh(u64 addr)
	 * a0, a1 hold low 32 and high 32
	 * v0     hold     16 of ret
	 */
	.global __raw__readh
	.ent	__raw__readh
__raw__readh:
	dsll32	a1, a1, 0
	dsll32	a0, a0, 0
	dsrl32	a0, a0, 0
	or	a0, a1, a0

	lh	v0, 0(a0)
	jr	ra
	sll	v0, v0, 0
.end	__raw__readh


	/* u64 __raw__writeh(u64 addr, u16 val)
	 * a0, a1 hold low 32 and high 32 of addr,
	 * a2     hold 16 of val,
	 * v0     hold 16 of ret
	 */
	.global __raw__writeh
	.ent	__raw__writeh
__raw__writeh:
	dsll32	a1, a1, 0
	dsll32	a0, a0, 0
	dsrl32	a0, a0, 0
	or	a0, a1, a0

	dsll32	a2, a2, 0
	dsrl32	a2, a2, 0

	sh	a2, 0(a0)
//	lh	v0, 0(a0)
	jr	ra
//	sll	v0, v0, 0
	nop
.end	__raw__writeh

	/* u64  __raw__readb(u64 addr)
	 * a0, a1 hold low 32 and high 32
	 * v0     hold     8 of ret
	 */
	.global __raw__readb
	.ent	__raw__readb
__raw__readb:
	dsll32	a1, a1, 0
	dsll32	a0, a0, 0
	dsrl32	a0, a0, 0
	or	a0, a1, a0

	lb	v0, 0(a0)
	jr	ra
	sll	v0, v0, 0
.end	__raw__readb

	/* u64 __raw__writeb(u64 addr, u8 val)
	 * a0, a1 hold low 32 and high 32 of addr,
	 * a2     hold 8 of val,
	 * v0     hold 8 of ret
	 */
	.global __raw__writeb
	.ent	__raw__writeb
__raw__writeb:
	dsll32	a1, a1, 0
	dsll32	a0, a0, 0
	dsrl32	a0, a0, 0
	or	a0, a1, a0

	dsll32	a2, a2, 0
	dsrl32	a2, a2, 0

	sb	a2, 0(a0)
//	lb	v0, 0(a0)
	jr	ra
//	sll	v0, v0, 0
	nop
.end	__raw__writeb
#endif //CONFIG_32BIT
